//
// Created by hoyin on 2023/12/25.
//

#ifndef VISUAL_LOCATION_COORDINATE_H
#define VISUAL_LOCATION_COORDINATE_H

#include "Eigen/Core"
using namespace Eigen;

class Coordinate {
protected:
	Vector3d XYZ;
public:
	double X() const;
	double Y() const;
	double Z() const;
	Vector3d getXYZ() const;
    void applyRotate(Matrix<double, 3, 3> R);
    void applyShift(Matrix<double, 3, 1> V);

    Coordinate() {
        XYZ = Vector3d::Zero();
    }
};

/**
 * 旋转矩阵
 */
class Rotation {
public:
	double phi;
	double omega;
	double kappa;
	Matrix3d R;
	/**
	 * 旋转矩阵构造函数
	 * @param phi
	 * @param omega
	 * @param kappa
	 */
	Rotation(double phi, double omega, double kappa) : phi(phi), omega(omega), kappa(kappa) {
		R = getRotationMatrix(phi, omega, kappa);
	}

	void setRotation(double phi, double omega, double kappa);
	void setRotation(Matrix3d R);


private:
	static Matrix3d getRotationMatrix(double phi, double omega, double kappa);
};

/**
 * 像空间坐标 S-xyz
 * S: 投影中心
 * x: 像片水平中线
 * y: 像片竖直中线
 * z: 垂直于像平面，等于 -f
 */
class ImageCoord : public Coordinate {
public:
    /**
     * 像空间坐标构造函数
     * @param xy 像素坐标，以左上角为原点，向右为x轴计算距离(mm)
     * @param intrinsicParams 内参，包括焦距和主点位置
     * 						主点位置，以左上角为原点，向右为x轴计算距离(mm)
     */
     /** 在像平面和像空间坐标系中，都是以主点为原点。 */
    ImageCoord(Vector2d &xy, const Vector3d &intrinsicParams) {
        double x = xy(0) - intrinsicParams(0);
        double y = xy(1) - intrinsicParams(1);
		double f = intrinsicParams(2);
        XYZ = Vector3d(x, y, -f);
    }
	ImageCoord(const ImageCoord& src) {
		double x = src.X();
		double y = src.Y();
		double z = src.Z();
		XYZ = Vector3d(x, y, z);
	}
	ImageCoord copy() {
		return ImageCoord(*this);
	}
};

/**
 * 像空间辅助坐标 O-XpYpZp
 * O：投影中心
 * Xp: 像片水平中线
 * Yp: 像片竖直中线
 * Zp: 垂直于地面向上
 */
class ImageAssistCoord : public Coordinate {
public:
	/**
	 * 像空间辅助坐标构造函数，使用像空间坐标和像空间系与像空辅系的旋转矩阵构造
	 * @param imageCoord 像空间坐标
	 * @param R 旋转矩阵
	 */
    ImageAssistCoord(ImageCoord & imageCoord, Matrix3d & R) {
        XYZ = R * imageCoord.getXYZ();
    }
};

class PhotographyCoord : public Coordinate {

	PhotographyCoord(Vector3d & XYZ) {
		this->applyShift(XYZ);
	}
};

#endif //VISUAL_LOCATION_COORDINATE_H
